#!/usr/bin/env python

import roslib; roslib.load_manifest('DelphinROSv2')
import rospy
import smach
import smach_ros

# GoToWaypoint class - CURRENTLY JUST A TEMPLATE!
# @extends: smach.State 
#
# __init__ - initialises a GoToWaypoint object
# @param: lib - instance of library_highlevel.py
#
# execute - waits for four seconds
# @return: preempted: if backseatdriver sets flag to 1 (e.g. from over-depth etc)
# @return: succeeded: otherwise

class GoToWaypoint(smach.State):
	def __init__(self, lib):
		smach.State.__init__(self, outcomes=['succeeded','aborted','preempted'])
                self.__controller = lib
  		
	def execute(self,userdata):
		#depth == 0
		for i in range(0,4):
                    if self.__controller.getBackSeatErrorFlag() == 0:
                        time.sleep(1)
                    else:
                        return 'preempted'
                return 'succeeded'